/*
 * maths.hpp
 *
 *  Created on: Jun 6, 2016
 *      Author: Yang
 *      Use Maths but not Math to avoid duplicate.
 */

#ifndef MATH_HPP_
#define MATH_HPP_

#include <cmath>
#include <stdint.h>

#include "Rotations.hpp"
#include "Vector2.hpp"
#include "Vector3.hpp"
#include "Matrix3.hpp"
#include "Quaternion.hpp"
#include "Definitions.hpp"

#define Yun_isinf(__x) ((sizeof(__x) == sizeof(float))  ? __isinff(__x) :  __isinfd(__x))
#define Yun_isnan(__x) ((sizeof(__x) == sizeof(float))  ? __isnanf(__x) :  __isnand(__x))

// are two floats equal
static inline bool is_equal(const float fVal1, const float fVal2)
{
    return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false;
}

// is a float is zero
static inline bool is_zero(const float fVal1)
{
    return fabsf(fVal1) < FLT_EPSILON ? true : false;
}

// a varient of asin() that always gives a valid answer.
float safe_asin(float v);

// a varient of sqrt() that always gives a valid answer.
float safe_sqrt(float v);

// square
static inline float sq(float v)
{
    return v * v;
}

// 2D vector length
static inline float pythagorous2(float a, float b)
{
    return sqrtf(sq(a) + sq(b));
}

// 3D vector length
static inline float pythagorous3(float a, float b, float c)
{
    return sqrtf(sq(a) + sq(b) + sq(c));
}

/*
  wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
 */
float wrap_PI(float angle_in_radians);

/*
  wrap an angle defined in radians to the interval [0,2*PI)
 */
float wrap_2PI(float angle);

// constrain a value
// constrain a value
static inline float constrain_float(float amt, float low, float high)
{
    // the check for NaN as a float prevents propogation of
    // floating point errors through any function that uses
    // constrain_float(). The normal float semantics already handle -Inf
    // and +Inf
    if (Yun_isnan(amt)) {
        return (low+high)*0.5f;
    }
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}

// degrees -> radians
static inline float radians(float deg) {
    return deg * DEG_TO_RAD;
}

// radians -> degrees
static inline float degrees(float rad) {
    return rad * RAD_TO_DEG;
}

#endif /* MATH_HPP_ */
